/**
 * @file GyroScope.cpp
 * @author 赵曦 (535394140@qq.com)
 * @brief 通信协议管理类
 * @version 1.0
 * @date 2021-06-14
 * 
 * @copyright Copyright SCUT RobotLab(c) 2021
 * 
 */

#include "GyroScope.h"

/**
 * @brief 构造函数
 */
GyroScope::GyroScope() {
    this->isInit = false;
}

#ifdef Infantry
/**
 * @brief 构造函数
 *
 * @param gyroScope 收到的数据向量
 */
GyroData GyroScope::filter(const vector<GyroData> &gyroScope) {
    // 判断是否初始化
    if (!this->isInit) {
        this->gyroData.pitch = Other_param.init_pitch;
        this->gyroData.yaw = Other_param.init_yaw;
        this->gyroData.color_mode = Other_param.init_color_mode;
        this->gyroData.shootSpeed = Other_param.init_shoot_speed;
        this->gyroData.mode = Other_param.init_detect_mode;
        // 更新标志位
        this->isInit = true;
    }
    // 判断是否收到数据
    if (!gyroScope.empty()) {
        GyroData tmp_data = gyroScope.back();
        // 异常判断
        if ((tmp_data.shootSpeed != 15 && tmp_data.shootSpeed != 18 && tmp_data.shootSpeed != 30) ||
            (tmp_data.mode > 5) || (tmp_data.color_mode != 0 && tmp_data.color_mode != 1)) {   //读取异常数据后直接返回上次读取的陀螺仪数据
            return this->gyroData;
        } else {
            this->gyroData = tmp_data;
        }
    }

    // 手动设置子弹射速，偏差修正
    switch (this->gyroData.shootSpeed) {
        case 15:
            this->gyroData.shootSpeed -= strategyParam.SHOOT_SPEED_15_BIAS;
            break;
        case 18:
            this->gyroData.shootSpeed -= strategyParam.SHOOT_SPEED_18_BIAS;
            break;
        case 30:
            this->gyroData.shootSpeed -= strategyParam.SHOOT_SPEED_30_BIAS;
            break;
        default:
            break;
    }
    return this->gyroData;
}
#endif

#ifdef Sentry

/**
 * @brief 过滤异常数据(哨兵)
 * @param gyroData
 * @return
 */
GyroData GyroScope::filter(const vector<GyroData> &gyroData) {
    // 判断是否初始化
    if (!this->isInit) {
        this->gyroData.pitch = Other_param.init_pitch;
        this->gyroData.yawSpeed = Other_param.init_yaw_speed;
        this->gyroData.SelfColor = Other_param.init_color_mode;
        this->gyroData.pitchSpeed = Other_param.init_pitch_speed;
        this->gyroData.RegionMode = Other_param.init_detect_mode;
        // 更新标志位
        this->isInit = true;
    }
    // 判断是否收到数据
    if (!gyroData.empty()) {
        GyroData tmp_data = gyroData.back();
        // 异常判断
        if (tmp_data.RegionMode != Region::Nolmal &&
            tmp_data.RegionMode != Region::HighLand) {   //读取异常数据后直接返回上次读取的陀螺仪数据
            return this->gyroData;
        } else {
            this->gyroData = tmp_data;
        }
    }
    return this->gyroData;
}

/**
 * @brief 发送数据赋值
 * @param dataStruct 待发送数据
 * @param displacement 目标偏移角
 */
void GyroScope::send(DataStruct &dataStruct,const cv::Point2f &displacement) {
    dataStruct.yaw = displacement.x * 100;
    dataStruct.pitch = displacement.y * 100;
    dataStruct.Flag = 72;
    dataStruct.shoot_mode = 64;
    dataStruct.shoot_rate = 0;
    dataStruct.End++;
    dataStruct.End = (dataStruct.End % 255);
}

#endif